﻿/////////////////////////////////////////////////////////////////
//
// Silverlight 5: 3D Physics using Balder and Jiglibx
// by Andy Beaulieu - http://www.andybeaulieu.com
//
// LICENSE: This code is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
// without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. ANY 
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS
// OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
/////////////////////////////////////////////////////////////////
using System;
using System.Net;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Documents;
using System.Windows.Ink;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Animation;
using System.Windows.Shapes;
using Balder.Objects.Geometries;
using Microsoft.Xna.Framework;
using JigLibX.Physics;
using JigLibX.Collision;
using jlxg = JigLibX.Geometry;
using JigLibX.Geometry;
using Balder;
using System.Collections.Generic;
using JigLibX.Vehicles;

namespace Spritehand.Physics3D
{
    public class PhysicsCar : IPhysicsObject
    {
        PhysicsSystem _world;
        private Car _car;
        List<Mesh> _wheelModels = new List<Mesh>();
        Mesh _carBody;
        const string _wheelModelPath = "Spritehand.Physics3D;component/content/wheel.ase";
        const string _carModelPath = "Spritehand.Physics3D;component/content/car.ase";



        public PhysicsCar(PhysicsSystem world, Container container, List<IPhysicsObject> physicsObjects, Balder.Math.Coordinate position)
        {
            const float wheelRadius = 0.4f;

            // create the wheel models
            for (int i = 0; i < 4; i++)
            {
                Mesh wheel = new Mesh();
                wheel.AssetName = new Uri(_wheelModelPath, UriKind.Relative);
                _wheelModels.Add(wheel);
                container.Children.Add(wheel as Balder.INode);
            }
            // create the car body model
            _carBody = new Mesh();
            _carBody.AssetName = new Uri(_carModelPath, UriKind.Relative);
            container.Children.Add(_carBody as Balder.INode);

            physicsObjects.Add(this);

            _world = world;

            _car = new Car(true, true, 30.0f, 5.0f, 4.7f, 5.0f, 0.20f, wheelRadius, 0.05f, 0.45f, 0.3f, 1, 520.0f, _world.Gravity.Length());

            _physicsBody = _car.Chassis.Body;
            _skin = _car.Chassis.Skin;

            SetCarMass(10.0f);

            _car.Chassis.Body.MoveTo(Utils.VectorBalderToXna(position), Microsoft.Xna.Framework.Matrix.Identity); // Microsoft.Xna.Framework.Matrix.CreateRotationY(MathHelper.ToRadians(90)));
            _car.EnableCar();
            _car.Chassis.Body.AllowFreezing = false;

        }

        public Mesh CarBody
        {
            get
            {
                return _carBody;
            }
        }


        private Body _physicsBody;
        public Body PhysicsBody()
        {
            return _physicsBody;
        }

        private CollisionSkin _skin;
        public CollisionSkin Skin
        {
            get
            {
                return _skin;
            }
        }


        private void SetCarMass(float mass)
        {
            _physicsBody.Mass = mass;
            Vector3 min, max;
            _car.Chassis.GetDims(out min, out max);
            Vector3 sides = max - min;

            float Ixx = (1.0f / 12.0f) * mass * (sides.Y * sides.Y + sides.Z * sides.Z);
            float Iyy = (1.0f / 12.0f) * mass * (sides.X * sides.X + sides.Z * sides.Z);
            float Izz = (1.0f / 12.0f) * mass * (sides.X * sides.X + sides.Y * sides.Y);

            Microsoft.Xna.Framework.Matrix inertia = Microsoft.Xna.Framework.Matrix.Identity;
            inertia.M11 = Ixx; inertia.M22 = Iyy; inertia.M33 = Izz;
            _car.Chassis.Body.BodyInertia = inertia;
            _car.SetupDefaultWheels();
        }


        public void Draw()
        {
            if (InputManager.IsKeyDown(Key.Up))
            {
                _car.Wheels[0].AddTorque(1000);
                _car.Wheels[1].AddTorque(1000);
                _car.Wheels[2].AddTorque(1000);
                _car.Wheels[3].AddTorque(1000);

                
            }

            DrawWheel(0, false);
            DrawWheel(1, false);
            DrawWheel(2, true);
            DrawWheel(3, true);

            // take care of the car body
            _carBody.Position = Utils.VectorXnaToBalder(_skin.NewPosition);

            Microsoft.Xna.Framework.Matrix matrixSrc = _physicsBody.CollisionSkin.GetPrimitiveLocal(0).Transform.Orientation * _physicsBody.Orientation;

            Balder.Math.Matrix matrix = Utils.MatrixXnaToBalder(matrixSrc);

            _carBody.World = matrix; 

        }

        private void DrawWheel(int wheelNum, bool rotated)
        {
            Wheel wh = _car.Wheels[wheelNum];

            Mesh mesh = _wheelModels[wheelNum];

            float steer = wh.SteerAngle;

            Microsoft.Xna.Framework.Matrix rot;
            if (rotated) rot = Microsoft.Xna.Framework.Matrix.CreateRotationY(MathHelper.ToRadians(180.0f));
            else rot = Microsoft.Xna.Framework.Matrix.Identity;





            mesh.Position = Utils.VectorXnaToBalder(_skin.NewPosition);

            Microsoft.Xna.Framework.Matrix matrixSrc = rot * Microsoft.Xna.Framework.Matrix.CreateRotationZ(MathHelper.ToRadians(-wh.AxisAngle)) * // rotate the wheels
                Microsoft.Xna.Framework.Matrix.CreateRotationY(MathHelper.ToRadians(steer)) *
                Microsoft.Xna.Framework.Matrix.CreateTranslation(wh.Pos + wh.Displacement * wh.LocalAxisUp) * _car.Chassis.Body.Orientation;

           

            Balder.Math.Matrix matrix = Utils.MatrixXnaToBalder(matrixSrc);


            mesh.World = matrix;

        }



    }
}
